program mainlrtest
use normalscr2
use matrixfuns
use datahadler
use suplmod
use mardiamod
implicit none
integer, parameter:: TT=1000,nn=3,S=10000,ngrid1=20,ngrid2=5
integer,parameter:: nvar=2*nn+5
integer iparam(7),T,n,i,j,i1,i2,sit,ind1(TT),ind2(nn),seedvec(S),seedind(S)

double precision:: y(TT,nn),thscale(nvar),rparam(7),a1,a2,c9

double precision:: thlb(nvar),thub(nvar),thtrue(nvar),therr(nvar)&
				 &,th(nvar),llik0,llik1,grad(nvar),tha(nvar),thb(nvar)&
				 &,thfix(nvar)

double precision:: mth(S,nvar),lr(S)

double precision:: thshape(nn+2),gra(nn+2),grb(nn+2),etagrid(ngrid1),sigrid(ngrid2)&
&,thshlb(nn+2),thshub(nn+2),thshape0(nn+2),llikf,llikj

double precision:: yi(TT,nn)
double precision, parameter:: pi=3.14159265358979d0,eta=.01d0,p_si=.999d0,b(3)=(/-5.0d-1,-5.0d-1,-5.0d-1/)
				 
common T,n,y

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
T=TT
n=nn
call rnset(1)
!!!!!!!!!!True parameters
a1=.1d0
a2=.85d0
c9=.999d0

thtrue(1:n)=vassign(n,.2d0)			!delta
thtrue(n+1:2*n-1)=vassign(n-1,1.0d0)	!c
thtrue(2*n)=0.05d0					!alpha0
thtrue(2*n+1)=0.05d0				!phi0
thtrue(2*n+2)=dasin(dsqrt(a1/c9))    !alpha1
thtrue(2*n+3)=dasin(dsqrt(a2/(c9-a1)))   	!alpha2
thtrue(2*n+4)=dasin(dsqrt(a1/c9))		!phi1
thtrue(2*n+5)=dasin(dsqrt(a2/(c9-a1)))	!phi2
!!!!!!!!! Limits

thlb(1:n)=vassign(n,-1.0d2)			   !delta
thlb(n+1:2*n-1)=vassign(n-1,-1.0d2)		   !c
thlb(2*n)=1.0d-10					   !alpha0
thlb(2*n+1)=1.0d-5					   !phi0
thlb(2*n+2:2*n+5)=vassign(4,0.0d0)

thub(1:n)=vassign(n,1.0d2)			   !delta
thub(n+1:2*n-1)=vassign(n-1,1.0d2)		   !c
thub(2*n)=1.0d2 					   !alpha0
thub(2*n+1)=1.0d2					   !phi0
thub(2*n+2:2*n+5)=vassign(4,2.0d0*pi)

thscale=vassign(nvar,1.0d0)

!!!!

!etagrid=linspace(-.2d0,.2d0,ngrid1)
!etagrid=linspace(.005d0,.1d0,ngrid1)
!sigrid=linspace(5.0d-2,.99d0,ngrid2)
thshlb=vassign(n+2,-1000.0d0)
thshub=vassign(n+2, 1000.0d0)
thshlb(1:2)=(/0.002d0,2.0d-2/)
thshub(1:2)=(/ 5.0d0,.999d0/)

!new
etagrid=linspace(.005d0,.1d0,ngrid1)
sigrid=linspace(0.1d0,.99d0,ngrid2)
!thshlb(1:2)=(/-5.0d0,5.0d-2/)
!thshub(1:2)=(/5.0d0,.99d0/)


!!!!
call readvecint(seedvec,S,"seedsim.txt",1)

forall(i=1:S)
	lr(i)=0.0d0
end forall


call readvecint(seedvec,S,"seedsim.txt",1)
call readvecint(seedind,2,"seedind.txt",1)

do sit=seedind(1),seedind(2),1  
print*,sit
call rnset(seedvec(sit))
!!!!!!!!!!Simulation
call ghsimul(nvar,T,n,thtrue,eta,b,p_si,y)



!!!!!!!!!!Estimation
call du4inf(iparam,rparam)
iparam(3)=1000
iparam(4)=1000
iparam(5)=1000
!rparam(1)=1.0d-7
!rparam(2)=1.0d-15
call dbcong(gallik,gascr,nvar,thtrue,0,thlb,thub&
          &,thscale,1.0d0,iparam,rparam,th,llik0)
mth(sit,:)=th

therr=dabs(thub-th)*dabs(th-thlb)
call dsvrgn(nvar,therr,therr)

if (therr(1)>0.0d0) then
	call givebin(nvar,T,n,y,th,thshape(3:n+2))
	thfix=th
	!thshape(1:2)=(/1.0d-3,0.999d0/)
	thshape(1:2)=(/.05d0,0.999d0/)
	call ghllik(n+2,thshape,llikf)
	thshape0=thshape

	do i=1,ngrid1,1
	do j=1,ngrid2,1
		thshape(1:2)=(/etagrid(i),sigrid(j)/)
		call ghllik(n+2,thshape,llikj)
		if (llikj<llikf) then
			thshape0=thshape
			llikf=llikj
		end if
	end do
	end do
	!!

	!!GRID
	call erset(0,0,0)
	call du4inf(iparam,rparam)
	iparam(3)=25
	iparam(4)=1000
	iparam(5)=1000
	call dbcong(ghllik,ghscr,n+2,thshape0,0,thshlb,thshub&
          &,thscale(1:n+2),1.0d0,iparam,rparam,thshape,llik1)

	lr(sit)=max(-2.0d0*(llik1-llik0),0.0d0)
	print*,sit,lr(sit)
	
	call exportvec(vec2(mth(seedind(1):sit,:)),"matth.txt",1)
	call exportvec(lr(seedind(1):sit),"lrtest.txt",1)
end if
end do
call exportvec(vec2(mth),"matth.txt",1)
call exportvec(lr,"lrtest.txt",1)

contains
subroutine ghllik(m,th,likresm)
!lambdat
use datahadler
use linear_operators
use matrixfuns
use bessels
use ghs
integer, intent(in):: m
double precision, intent(in):: th(m)
double precision, intent(out):: likresm
integer i,j
double precision, parameter:: pi=3.14159265358979d0
double precision lamb(T),delta(n),c(n),phi0(n),phi1(n),phi2(n),&
                &alpha0,alpha1,alpha2,eta,si,b(n),llik,sigmat(n,n),gamma(n),gammat(n,n),&
				&cc(n,n),epst(1:T,1:n),isigmat(n,n),igammat(n,n)&
				&,fttm,wttm,nufun,gamfun,besrat,bb,cfun,lt(n)&
				&,sigmastar(n,n),isigmastar(n,n),a,bbp(n,n),alpha,qfun&
				&,besr,besrp,besrm,d


call exportvec(th,"thghint.txt",5)

delta=thfix(1:n)
c(1)=1.0d0
c(2:n)=thfix(n+1:2*n-1)
alpha0=thfix(2*n)
phi0=vassign(n,thfix(2*n+1))
alpha1=c9*(dsin(thfix(2*n+2))**2.0d0)
alpha2=(c9-alpha1)*(dsin(thfix(2*n+3))**2.0d0)

forall(i=1:n)
phi1(i)=c9*(dsin(thfix(2*n+4))**2.0d0)
end forall
forall(i=1:n)
phi2(i)=(c9-phi1(i))*(dsin(thfix(2*n+5))**2.0d0)
end forall


if (th(1)>0.0d0) then
	eta=1.01d-3+th(1)
else
	eta=-1.01d-3+th(1)
end if
si=th(2)
b=th(3:2+n)


nufun=-1.0d0/(2.0d0*eta)
gamfun=(1.0d0-si)/si

besrat=besselr(nufun,gamfun)/gamfun



forall(i=1:n,j=1:n)
	cc(i,j)=c(i)*c(j)
	bbp(i,j)=b(i)*b(j)
end forall

forall(i=1:T,j=1:n)
	epst(i,j)=y(i,j)-delta(j)
end forall

!!
gamma=phi0/(1.0d0-phi1-phi2)
gammat=diag(gamma)
igammat=diag(1.0d0/gamma)

lamb(1)=alpha0/(1.0d0-alpha1-alpha2)
sigmat=(cc*lamb(1))+gammat
isigmat=igammat-lamb(1)*(((igammat.x.cc).x.igammat)&
     &/(lamb(1)*dot_product(c,matmul(igammat,c))+1.0d0))
!!
bb=dot_product(b,matmul(sigmat,b))

cfun=ghcfun1(bb,eta,si)
lt=epst(1,:)+cfun*matmul(sigmat,b)

if (bb>1.0d-8) then
    isigmastar=isigmat-((cfun-1.0d0)/(cfun*bb))*bbp
    sigmastar=sigmat+((cfun-1.0d0)/bb)*((sigmat.x.bbp).x.sigmat)
else
    a=besseld(nufun+1.0d0,gamfun)-1.0d0
    isigmastar=isigmat-((-a+2.0d0*a*a*bb*bb-5.0d0*(a**3.0d0)*(bb**2.0d0))/cfun)*bbp
    sigmastar=sigmat+(-a+2.0d0*(a**2.0d0)*(bb**2.0d0)-5.0d0*(a**3.0d0)*(bb**2.0d0))&
	         &*((sigmat.x.bbp).x.sigmat)
end if

qfun=dsqrt(1.0d0+besrat*dot_product(lt,matmul(isigmastar,lt)))

alpha=dsqrt((cfun*bb/besrat)+gamfun*gamfun)

!!

llik=nufun*dlog(gamfun)+.5d0*n*dlog(besrat)-.5d0*n*dlog(2.0d0*pi)&
     &-2.0d0*(nufun-.5d0*n)*dlog(alpha)-robustlbesk(nufun,gamfun)-.5*dlog(det(sigmastar))&
	 &+(nufun-.5d0*n)*dlog(alpha*qfun)+robustlbesk(nufun-.5d0*n,alpha*qfun)+dot_product(b,lt)


do i=2,T,1
	wttm=lamb(i-1)/(1.0d0+lamb(i-1)*dot_product(c,c/gamma))
	fttm=wttm*dot_product((c/gamma),epst(i-1,:))
	lamb(i)=alpha0&
	       &+alpha1*(((fttm)**2.0d0)+wttm)+alpha2*lamb(i-1)


	gamma=phi0+phi1*(((epst(i-1,:)-c*fttm)**2.0d0)+(c*c)*wttm)+phi2*gamma

	gammat=diag(gamma)

	igammat=diag(1.0d0/gamma)

	sigmat=(cc*lamb(i))+gammat
	isigmat=igammat-lamb(i)*(((igammat.x.cc).x.igammat)&
	     &/(lamb(i)*dot_product(c,matmul(igammat,c))+1.0d0))

	!!
	
	bb=dot_product(b,matmul(sigmat,b))
	cfun=ghcfun1(bb,eta,si)
	lt=epst(i,:)+cfun*matmul(sigmat,b)

	if (bb>1.0d-8) then
	    isigmastar=isigmat-((cfun-1.0d0)/(cfun*bb))*bbp
	    sigmastar=sigmat+((cfun-1.0d0)/bb)*((sigmat.x.bbp).x.sigmat)
	else
	    a=besseld(nufun+1.0d0,gamfun)-1.0d0
	    isigmastar=isigmat-((-a+2.0d0*a*a*bb*bb-5.0d0*(a**3.0d0)*(bb**2.0d0))/cfun)*bbp
	    sigmastar=sigmat+(-a+2.0d0*(a**2.0d0)*(bb**2.0d0)-5.0d0*(a**3.0d0)*(bb**2.0d0))&
		         &*((sigmat.x.bbp).x.sigmat)
	end if

		
	qfun=dsqrt(1.0d0+besrat*dot_product(lt,matmul(isigmastar,lt)))
	alpha=dsqrt((cfun*bb/besrat)+gamfun*gamfun)

	!!
	llik=llik+nufun*dlog(gamfun)+.5d0*n*dlog(besrat)-.5d0*n*dlog(2.0d0*pi)&
	     &-2.0d0*(nufun-.5d0*n)*dlog(alpha)-robustlbesk(nufun,gamfun)-.5*dlog(det(sigmastar))&	
		 &+(nufun-.5d0*n)*dlog(alpha*qfun)+robustlbesk(nufun-.5d0*n,alpha*qfun)+dot_product(b,lt)

end do
likresm=-llik

!print*, real(likresm),real(eta),real(si),real(minval(b)),real(maxval(b))

end subroutine ghllik

subroutine ghscr(m,th,grad)
use linear_operators
use matrixfuns
use datahadler
use bessels
use dbessels
use ghs
integer, intent(in):: m
double precision, intent(in):: th(m)
double precision, intent(out):: grad(m)
integer i,j,ii,jj
double precision lamb(T),delta(n),c(n),phi0(n),phi1(n),phi2(n),rnu(n),gamma(n),&
                &gammat(n,n),alpha0,alpha1,alpha2,cc(n,n),epst(1:T,1:n),isigmat(n,n)&
				&,igammat(n,n),wttm,fttm,isigmastar(n,n),a,bbp(n,n),dlgKsi,besr,dlgRsi &
				&,besrp,besrm,d,dDsi,deriv,deriv1,dlgKeta,dlgReta,dDeta,epsti(n)&
				&,p1,p2,ff,gg,coefcfun,coef2,dcfuneta,dcfunsi,elogsi,sigb(n),ghgradi(m)&
				&,lami,dlgKeta2,dlgReta2,dDeta2,bb,b(nn),sigmat(nn,nn),eta,si,nufun,gamfun &
				&,besrat,cfun,lt(n),rt,ltl


delta=thfix(1:n)
c(1)=1.0d0
c(2:n)=thfix(n+1:2*n-1)
alpha0=thfix(2*n)
phi0=vassign(n,thfix(2*n+1))
alpha1=c9*(dsin(thfix(2*n+2))**2.0d0)
alpha2=(c9-alpha1)*(dsin(thfix(2*n+3))**2.0d0)

forall(i=1:n)
phi1(i)=c9*(dsin(thfix(2*n+4))**2.0d0)
end forall
forall(i=1:n)
phi2(i)=(c9-phi1(i))*(dsin(thfix(2*n+5))**2.0d0)
end forall


if (th(1)>0.0d0) then
	eta=1.01d-3+th(1)
else
	eta=-1.01d-3+th(1)
end if
si=th(2)
b=th(3:2+n)



nufun=-1.0d0/(2.0d0*eta)
gamfun=(1.0d0-si)/si
besrat=besselr(nufun,gamfun)/gamfun


dlgKsi=0.5d0*(1.0d0/si**2.0d0)*((1.0d0/besselr(nufun-1.0d0,gamfun))+besselr(nufun,gamfun))

besr=besselr(nufun,gamfun)
dlgRsi=-(1.0d0/si**2.0d0)*(besr-((2.0d0*nufun+1.0d0)/gamfun)-(1.0d0/besr))

besrp=besselr(nufun+1.0d0,gamfun)
besrm=besselr(-nufun-1.0d0,gamfun)
d=besseld(nufun+1.0d0,gamfun)
dDsi=d*(besrp-(1.0d0/besrp)+besrm-(1.0d0/besrm)-(2.0d0/gamfun))*(-1.0d0/si**2.0d0)

a=besseld(nufun+1.0d0,gamfun)-1.0d0
deriv=vdlogk(nufun,gamfun,1.0d-8)
deriv1=vdlogk(nufun+1.0d0,gamfun,1.0d-8)

dlgKeta=deriv/(2.0d0*eta**2.0d0)
dlgReta=(deriv1-deriv)/(2.0d0*eta**2.0d0)
dDeta=besseld(nufun+1.0d0,gamfun)*(vdlogk(nufun+2.0d0,gamfun,1.0d-8)+deriv-2.0d0*deriv1)/(2.0d0*eta**2.0d0)


forall(i=1:T,j=1:n)
	epst(i,j)=y(i,j)-delta(j)
end forall


forall(i=1:n,j=1:n)
	cc(i,j)=c(i)*c(j)
	bbp(i,j)=b(i)*b(j)
end forall


gamma=phi0/(1.0d0-phi1-phi2)
gammat=diag(gamma)
igammat=diag(1.0d0/gamma)

lamb(1)=alpha0/(1.0d0-alpha1-alpha2)
sigmat=(cc*lamb(1))+gammat
isigmat=igammat-lamb(1)*(((igammat.x.cc).x.igammat)&
     &/(lamb(1)*dot_product(c,matmul(igammat,c))+1.0d0))


i=1
epsti=epst(1,:)
lami=lamb(1)


!!!!!!!!!!!!!!!!!!!!!!!!
bb=dot_product(b,matmul(sigmat,b))
cfun=ghcfun1(bb,eta,si)
lt=epsti+cfun*matmul(sigmat,b)
rt=dsqrt(1.0d0+4.0d0*a*bb)

if (bb > 1.0d-8) then
    coefcfun=(cfun-1.0d0)/(bb)
    coef2=coefcfun*((1.0d0/rt)-1.0d0)/bb
else
    coefcfun=-a+2.0d0*a**2.0d0*bb-5.0d0*a**3.0d0*bb**2.0d0+14.0d0*a**4.0d0*bb**3.0d0
    coef2=coefcfun*(-2.0d0*a+6.0d0*a**2.0d0*bb-20.0d0*a**3.0d0*bb**2.0d0+70.0d0*a**4.0d0*bb**3.0d0)
end if
isigmastar=isigmat-(coefcfun/cfun)*bbp

ltl=dot_product(lt,matmul(isigmastar,lt))

p1=dsqrt((bb/besrat)*cfun+gamfun**2.0d0)
p2=dsqrt(besrat*ltl+1.0d0)


ff=besrat*(p1/p2)*besselr(.5d0*n-nufun,p1*p2)
gg=p2/(p1*besselr(.5d0*n-nufun-1.0d0,p1*p2)*besrat)



!! eta
dcfuneta=((cfun-1.0d0)/(a*rt))*dDeta
elogsi=dlog(p1/p2)+vdlogk(.5d0*n-nufun,p1*p2,1.0d-8)


ghgradi(1)=.5d0*n*dlgReta+(bb-(.5d0/cfun))*dcfuneta+(dlog(gamfun)/(2.0d0*eta**2.0d0))&
       &-dlgKeta-(1.0d0/(2.0d0*eta**2.0d0))*elogsi &
       &-.5d0*ff*(dlgReta*ltl+dcfuneta*bb&
	   &-(dot_product(b,epsti)**2.0d0*coefcfun*dDeta/(cfun**2.0d0*a*rt)))&
       &-.5d0*bb*gg*(dcfuneta-cfun*dlgReta)



!! si
dcfunsi=((cfun-1.0d0)/(a*rt))*dDsi
ghgradi(2)=.5d0*n*dlgRsi+(n/(2.0d0*si*(1.0d0-si)))&
     &+(bb-(.5d0/cfun))*dcfunsi+(1.0d0/(2.0d0*eta*si*(1.0d0-si)))-dlgKsi&
     &-.5d0*ff*((dlgRsi+(1.0d0/(si*(1.0d0-si))))*ltl&
	 &+bb*dcfunsi-(coefcfun*dot_product(b,epsti)**2.0d0*dDsi/(cfun**2.0d0*a*rt)))&
     &-.5d0*bb*gg*(-(cfun/(si*(1.0d0-si)))+dcfunsi-cfun*dlgRsi)&
	 &+gg*(besr/si**2.0d0)

!! b
sigb=matmul(sigmat,b)
ghgradi(3:2+n)=-(coefcfun/(cfun*rt))*sigb-ff*cfun*lt+epsti &
     &+ff*coefcfun*dot_product(b,lt)*((coefcfun*dot_product(b,lt)/(cfun**2.0d0*rt))*sigb&
	 &+(lt/cfun)-(sigb/rt))&
     &+((2.0d0-gg)/rt)*sigb
!!!!!!!!!!!!!!!!!!!!!!!!

grad=ghgradi


!t>1
do i=2,T,1
	wttm=lamb(i-1)/(1.0d0+lamb(i-1)*dot_product(c,c/gamma))
	fttm=wttm*dot_product((c/gamma),epst(i-1,:))
	lamb(i)=alpha0&
	       &+alpha1*(((fttm)**2.0d0)+wttm)+alpha2*lamb(i-1)


	gamma=phi0+phi1*(((epst(i-1,:)-c*fttm)**2.0d0)+(c*c)*wttm)+phi2*gamma

	gammat=diag(gamma)

	igammat=diag(1.0d0/gamma)

	sigmat=(cc*lamb(i))+gammat
	isigmat=igammat-lamb(i)*(((igammat.x.cc).x.igammat)&
	     &/(lamb(i)*dot_product(c,matmul(igammat,c))+1.0d0))
		
	lami=lamb(i)
	epsti=epst(i,:)
!!!!!!!!!!!!!!!!!!!!!!!!
bb=dot_product(b,matmul(sigmat,b))
cfun=ghcfun1(bb,eta,si)
lt=epsti+cfun*matmul(sigmat,b)
rt=dsqrt(1.0d0+4.0d0*a*bb)

if (bb > 1.0d-8) then
    coefcfun=(cfun-1.0d0)/(bb)
    coef2=coefcfun*((1.0d0/rt)-1.0d0)/bb
else
    coefcfun=-a+2.0d0*a**2.0d0*bb-5.0d0*a**3.0d0*bb**2.0d0+14.0d0*a**4.0d0*bb**3.0d0
    coef2=coefcfun*(-2.0d0*a+6.0d0*a**2.0d0*bb-20.0d0*a**3.0d0*bb**2.0d0+70.0d0*a**4.0d0*bb**3.0d0)
end if
isigmastar=isigmat-(coefcfun/cfun)*bbp

ltl=dot_product(lt,matmul(isigmastar,lt))

p1=dsqrt((bb/besrat)*cfun+gamfun**2.0d0)
p2=dsqrt(besrat*ltl+1.0d0)


ff=besrat*(p1/p2)*besselr(.5d0*n-nufun,p1*p2)
gg=p2/(p1*besselr(.5d0*n-nufun-1.0d0,p1*p2)*besrat)



!! eta
dcfuneta=((cfun-1.0d0)/(a*rt))*dDeta
elogsi=dlog(p1/p2)+vdlogk(.5d0*n-nufun,p1*p2,1.0d-8)


ghgradi(1)=.5d0*n*dlgReta+(bb-(.5d0/cfun))*dcfuneta+(dlog(gamfun)/(2.0d0*eta**2.0d0))&
       &-dlgKeta-(1.0d0/(2.0d0*eta**2.0d0))*elogsi &
       &-.5d0*ff*(dlgReta*ltl+dcfuneta*bb&
	   &-(dot_product(b,epsti)**2.0d0*coefcfun*dDeta/(cfun**2.0d0*a*rt)))&
       &-.5d0*bb*gg*(dcfuneta-cfun*dlgReta)



!! si
dcfunsi=((cfun-1.0d0)/(a*rt))*dDsi
ghgradi(2)=.5d0*n*dlgRsi+(n/(2.0d0*si*(1.0d0-si)))&
     &+(bb-(.5d0/cfun))*dcfunsi+(1.0d0/(2.0d0*eta*si*(1.0d0-si)))-dlgKsi&
     &-.5d0*ff*((dlgRsi+(1.0d0/(si*(1.0d0-si))))*ltl&
	 &+bb*dcfunsi-(coefcfun*dot_product(b,epsti)**2.0d0*dDsi/(cfun**2.0d0*a*rt)))&
     &-.5d0*bb*gg*(-(cfun/(si*(1.0d0-si)))+dcfunsi-cfun*dlgRsi)&
	 &+gg*(besr/si**2.0d0)

!! b
sigb=matmul(sigmat,b)
ghgradi(3:2+n)=-(coefcfun/(cfun*rt))*sigb-ff*cfun*lt+epsti &
     &+ff*coefcfun*dot_product(b,lt)*((coefcfun*dot_product(b,lt)/(cfun**2.0d0*rt))*sigb&
	 &+(lt/cfun)-(sigb/rt))&
     &+((2.0d0-gg)/rt)*sigb
!!!!!!!!!!!!!!!!!!!!!!!!

	grad=grad+ghgradi
end do

grad=-grad
end subroutine ghscr
end program mainlrtest